﻿using System;

namespace Grimgine.Math
{
    /// <summary>
    /// 
    /// </summary>
    public struct Quaternion : IEquatable<Quaternion>
    {
        #region Fields

        /// <summary>
        /// 
        /// </summary>
        public float X, Y, Z, W;

        /// <summary>
        /// 
        /// </summary>
        private static Quaternion identity = new Quaternion(0f, 0f, 0f, 1f);

        /// <summary>
        /// 
        /// </summary>
        private static Quaternion zero = new Quaternion(0f, 0f, 0f, 0f);

        #endregion

        #region Properties

        /// <summary>
        /// 
        /// </summary>
        public static Quaternion Identity
        {
            get { return identity; }
        }

        /// <summary>
        /// 
        /// </summary>
        public static Quaternion Zero
        {
            get { return zero; }
        }

        #endregion

        #region Constructors

        /// <summary>
        /// 
        /// </summary>
        /// <param name="w"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        public Quaternion(float w, float x, float y, float z)
        {
            W = w;
            X = x;
            Y = y;
            Z = z;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="axis"></param>
        /// <param name="angle"></param>
        public Quaternion(Vector3 axis, float angle)
        {
            // if axis of rotation is zero vector, just set identity quaternion
            float length = axis.LengthSquared();
            if (length == 0f)
            {
                throw new DivideByZeroException();
            }

            // take half-angle
            angle *= 0.5f;

            float sintheta, costheta;
            sintheta = MathHelper.Sin(angle);
            costheta = MathHelper.Cos(angle);

            float scaleFactor = sintheta / MathHelper.Sqrt(length);

            W = costheta;
            X = scaleFactor * axis.X;
            Y = scaleFactor * axis.Y;
            Z = scaleFactor * axis.Z;
        }

        #endregion

        #region Quaternion Arithmetic

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat"></param>
        /// <returns></returns>
        public static Quaternion operator -(Quaternion quat)
        {
            return new Quaternion(-quat.W, -quat.X, -quat.Y, -quat.Z);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat1"></param>
        /// <param name="quat2"></param>
        /// <returns></returns>
        public static Quaternion operator -(Quaternion quat1, Quaternion quat2)
        {
            float w = quat1.W - quat2.W;
            float x = quat1.X - quat2.X;
            float y = quat1.Y - quat2.Y;
            float z = quat1.Z - quat2.Z;

            return new Quaternion(w, x, y, z);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat"></param>
        /// <param name="scaleFactor"></param>
        /// <returns></returns>
        public static Quaternion operator *(Quaternion quat, float scaleFactor)
        {
            float w = quat.W * scaleFactor;
            float x = quat.X * scaleFactor;
            float y = quat.Y * scaleFactor;
            float z = quat.Z * scaleFactor;

            return new Quaternion(w, x, y, z);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat1"></param>
        /// <param name="quat2"></param>
        /// <returns></returns>
        public static Quaternion operator *(Quaternion quat1, Quaternion quat2)
        {
            return new Quaternion(
                quat1.W * quat2.W - quat1.X * quat2.X - quat1.Y * quat2.Y - quat1.Z * quat2.Z,
                quat1.W * quat2.X + quat1.X * quat2.W + quat1.Y * quat2.Z - quat1.Z * quat2.Y,
                quat1.W * quat2.Y + quat1.Y * quat2.W + quat1.Z * quat2.X - quat1.X * quat2.Z,
                quat1.W * quat2.Z + quat1.Z * quat2.W + quat1.X * quat2.Y - quat1.Y * quat2.X
            );
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat1"></param>
        /// <param name="quat2"></param>
        /// <returns></returns>
        public static Quaternion operator /(Quaternion quat1, Quaternion quat2)
        {
            return new Quaternion(
                quat1.W / quat2.W - quat1.X / quat2.X - quat1.Y / quat2.Y - quat1.Z / quat2.Z,
                quat1.W / quat2.X + quat1.X / quat2.W + quat1.Y / quat2.Z - quat1.Z / quat2.Y,
                quat1.W / quat2.Y + quat1.Y / quat2.W + quat1.Z / quat2.X - quat1.X / quat2.Z,
                quat1.W / quat2.Z + quat1.Z / quat2.W + quat1.X / quat2.Y - quat1.Y / quat2.X
            );
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat1"></param>
        /// <param name="quat2"></param>
        /// <returns></returns>
        public static Quaternion operator +(Quaternion quat1, Quaternion quat2)
        {
            float w = quat1.W + quat2.W;
            float x = quat1.X + quat2.X;
            float y = quat1.Y + quat2.Y;
            float z = quat1.Z + quat2.Z;

            return new Quaternion(w, x, y, z);
        }

        #endregion

        #region Quaternion Methods

        /// <summary>
        /// 
        /// </summary>
        public void Normalise()
        {
            float lengthSq = W * W + X * X + Y * Y + Z * Z;

            if (lengthSq == 0)
            {
                this = Quaternion.Zero;
            }
            else
            {
                float factor = MathHelper.InvSqrt(lengthSq);
                W *= factor;
                X *= factor;
                Y *= factor;
                Z *= factor;
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public float Magnitude()
        {
            return MathHelper.Sqrt(W * W + X * X + Y * Y + Z * Z);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat"></param>
        /// <returns></returns>
        public static Quaternion Conjugate(Quaternion quat)
        {
            return new Quaternion(quat.W, -quat.X, -quat.Y, -quat.Z);
        }

        /// <summary>
        /// 
        /// </summary>
        public void Conjugate()
        {
            X = -X;
            Y = -Y;
            Z = -Z;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="quat1"></param>
        /// <param name="quat2"></param>
        /// <returns></returns>
        public static float Dot(Quaternion quat1, Quaternion quat2)
        {
            return (quat1.W * quat2.W + quat1.X * quat2.X + quat1.Y * quat2.Y + quat1.Z * quat2.Z);
        }

        #endregion

        #region Comparison functions

        /// <summary>
        /// 
        /// </summary>
        /// <param name="q1"></param>
        /// <param name="q2"></param>
        /// <returns></returns>
        public static bool operator ==(Quaternion q1, Quaternion q2)
        {
            return q1.W == q2.W && q1.X == q2.X && q1.Y == q2.Y && q1.Z == q2.Z;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="q1"></param>
        /// <param name="q2"></param>
        /// <returns></returns>
        public static bool operator !=(Quaternion q1, Quaternion q2)
        {
            return !(q1 == q2);
        }

        #endregion

        #region IEquatable Implementation

        /// <summary>
        /// 
        /// </summary>
        /// <param name="obj"></param>
        /// <returns></returns>
        public override bool Equals(object obj)
        {
            return obj is Quaternion && this == (Quaternion)obj;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="other"></param>
        /// <returns></returns>
        public bool Equals(Quaternion other)
        {
            return (this == other);
        }

        #endregion

        #region Hash Code

        /// <summary>
        /// 
        /// </summary>
        /// <returns></returns>
        public override int GetHashCode()
        {
            return (int)((W + X + Y + Z) % Int32.MaxValue);
        }

        #endregion
    }
}
